﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace ProjectRoughWork
{
    class PathFollowing : AIState
    {
        /// <summary>
        /// 2-Parameter Constructor
        /// </summary>
        /// <param name="actorID">The ID of the actor to be manipulated</param>
        /// <param name="targetPath">A queue of nodes to follow to the goal node</param>

        public PathFollowing(SquadActor initActor, Queue<Node> targetPath) : base()
        {
            path = targetPath;

            mTarget = path.Dequeue().Position;

            actor = initActor;

            reachedEnd = false;
        }

        public override void VOnUpdate()
        {

            if (CheckIfArrived(actor.getPosition()))
            {

                if (path.Count > 0)
                {
                    mTarget = path.Dequeue().Position;
                    alligned = CheckIfAlligned(actor.getPosition(), actor.getRotation());
                }
                else
                {
                    reachedEnd = true;
                    EventCoverCheck checkEvt = new EventCoverCheck(actor);
                    EventManager.GetInstance().VQueueEvent(checkEvt);
                }
            }

            if (!reachedEnd)
            {
                if (alligned)
                {
                    FollowPath(actor);
                }
                else
                {
                    Allign(actor);
                }
            }
        }

        /// <summary>
        /// Checks if the actor has arrived at the current node in the path
        /// </summary>
        /// <param name="actorPosition">Current Position of the node</param>
        /// <returns>Returns true if arrived, false if the actor hasn't</returns>

        public bool CheckIfArrived(Vector3 actorPosition)
        {
            if ((actorPosition.X < mTarget.X + 20) && (actorPosition.X > mTarget.X - 20) &&
                    (actorPosition.Z < mTarget.Z + 20) && (actorPosition.Z > mTarget.Z - 20)
                )
            {
                actor.setVelocity(Vector3.Zero);
                return true;
            }

            return false;
        }

        public bool ReachedEnd
        {
            get { return reachedEnd; }
        }

        /// <summary>
        /// Tests if actor is alligned with target
        /// </summary>
        /// <param name="actorPosition">The actor's current position</param>
        /// <param name="currentRotation">The actor's current rotation</param>
        /// <returns>True if alligned with target, false if not</returns>

        public bool CheckIfAlligned(Vector3 actorPosition, float currentRotation)
        {
            Vector3 targetDir = mTarget - actorPosition;
            float necessaryRotation = -(float)Math.Atan2((double)targetDir.Z, (double)targetDir.X);

            if (necessaryRotation == currentRotation)
            {
                return true;
            }

            return false;
        }

        /// <summary>
        /// Makes the necessaary calculations to steer the actor towards the current target
        /// </summary>
        /// <param name="actor">The actor be steered/moved</param>

        public void FollowPath(SquadActor actor)
        {
            Vector3 acceleration;

            acceleration = mTarget - actor.getPosition();
            acceleration.Normalize();

            acceleration *= actor.MaxAcceleration;

            actor.setVelocity(actor.getVelocity() + (acceleration- actor.getVelocity()));
            //ScreenManager.GetInstance().MoveActor(mActorID, actor.getPosition() + actor.getVelocity()); Replace with Event

            //EventMoveActor moveEvent = new EventMoveActor(actor.getID(), actor.getPosition() + actor.getVelocity());
            //EventManager.GetInstance().VQueueEvent(moveEvent);
        }

        /// <summary>
        /// Calculates the necessary rotation of the actor to face its target
        /// </summary>
        /// <param name="actor"></param>

        public void Allign(SquadActor actor)
        {
            Vector3 targetDir = mTarget - actor.getPosition();
            float necessaryRotation = -(float)Math.Atan2((double)targetDir.Z, (double)targetDir.X);

            float correctedRotation = actor.NormalizeRotation(necessaryRotation - actor.getRotation());

            correctedRotation = MathHelper.Clamp(correctedRotation, -0.1f, 0.1f);

            actor.setRotation(actor.NormalizeRotation(actor.getRotation() + correctedRotation));
            actor.setVelocity(Vector3.Zero);

            alligned = CheckIfAlligned(actor.getPosition(), actor.getRotation());
             
        }

        public Vector3 CancelPathFind()
        {
            while (path.Count > 1)
            {
                path.Dequeue();
            }

            actor.setVelocity(Vector3.Zero);

            if (path.Count == 1)
            {
                return path.Dequeue().Position;
            }
            else
            {
                return mTarget;
            }
        }

        //Data Members

        SquadActor actor;
        Queue<Node> path;
        bool alligned;
        bool reachedEnd;
    }
}
